import time
import threading
from config import logger, MAX_SPEED, MIN_SPEED, SERVO_SPEED, SERVO1_INIT_ANGLE, SERVO2_INIT_ANGLE
from joystick import InputSmoother
# 导入树莓派机器人控制库
try:
    from Raspbot_Lib import Raspbot
    raspbot_available = True
except ImportError:
    print("警告: Raspbot_Lib未找到")
    raspbot_available = False

class RobotController:
    def __init__(self):
        self.servo1_current_angle = SERVO1_INIT_ANGLE
        self.servo2_current_angle = SERVO2_INIT_ANGLE
        self.control_thread_running = False
        self.control_thread = None
        self.latest_joystick = None
        self.robot = None
        self.input_smoother = InputSmoother()
        
        # 初始化机器人
        if raspbot_available:
            try:
                self.robot = Raspbot()
                logger.info("成功初始化Raspbot控制器")
            except Exception as e:
                logger.error(f"初始化Raspbot失败: {str(e)}")
                
    def map_joystick_to_motor(self, left_x, left_y, right_x, right_y):
        """
        将手柄输入映射为电机控制命令
        """
        # 如果左摇杆X和Y值都小于阈值，直接停车
        if abs(left_x) < 0.2 and abs(left_y) < 0.2:  # 修改阈值为0.2
            return 0, 0
            
        # 将Y轴值映射为前进/后退速度
        forward_speed = -left_y * MAX_SPEED  # 注意：通常摇杆向前是负Y值
        
        # 将X轴值映射为转向强度 - 修改此处，将正负号反转
        turn_value = -left_x * MAX_SPEED  # 添加负号来反转方向
        
        # 计算左右轮的速度
        left_speed = forward_speed - turn_value
        right_speed = forward_speed + turn_value
        
        # 限制速度在有效范围内
        left_speed = max(min(left_speed, MAX_SPEED), -MAX_SPEED)
        right_speed = max(min(right_speed, MAX_SPEED), -MAX_SPEED)
        
        # 应用死区 - 如果速度太小则归零
        if abs(left_speed) < MIN_SPEED:
            left_speed = 0
        if abs(right_speed) < MIN_SPEED:
            right_speed = 0
        
        return int(left_speed), int(right_speed)
    
    def control_robot_thread(self):
        """控制机器人的后台线程"""
        logger.info("机器人控制线程已启动")
        last_command_time = 0
        timeout = 0.5  # 0.5秒无数据则停车
        
        # 首次运行时初始化舵机位置
        if raspbot_available and self.robot:
            try:
                self.robot.Ctrl_Servo(1, self.servo1_current_angle)
                self.robot.Ctrl_Servo(2, self.servo2_current_angle)
                logger.info(f"舵机初始化: 1号={self.servo1_current_angle}, 2号={self.servo2_current_angle}")
            except Exception as e:
                logger.error(f"舵机初始化出错: {str(e)}")
        
        while self.control_thread_running:
            if self.latest_joystick:
                current_time = time.time()
                joystick_time = self.latest_joystick.get('timestamp', 0)
                
                # 检查数据是否过期
                if current_time - joystick_time < timeout:
                    # 提取摇杆值
                    left_x = self.latest_joystick.get('left_x', 0)
                    left_y = self.latest_joystick.get('left_y', 0)
                    right_x = self.latest_joystick.get('right_x', 0)
                    right_y = self.latest_joystick.get('right_y', 0)
                    
                    # 映射为电机控制命令
                    left_speed, right_speed = self.map_joystick_to_motor(left_x, left_y, right_x, right_y)
                    
                    # 记录命令
                    logger.info(f"控制命令: 左={left_speed}, 右={right_speed}")
                    
                    # 控制机器人
                    if raspbot_available and self.robot:
                        try:
                            # 左侧电机 (ID 0, 1)
                            if left_speed >= 0:
                                self.robot.Ctrl_Car(0, 0, left_speed)  # 前进
                                self.robot.Ctrl_Car(1, 0, left_speed)
                            else:
                                self.robot.Ctrl_Car(0, 1, abs(left_speed))  # 后退
                                self.robot.Ctrl_Car(1, 1, abs(left_speed))
                                
                            # 右侧电机 (ID 2, 3)
                            if right_speed >= 0:
                                self.robot.Ctrl_Car(2, 0, right_speed)  # 前进
                                self.robot.Ctrl_Car(3, 0, right_speed)
                            else:
                                self.robot.Ctrl_Car(2, 1, abs(right_speed))  # 后退
                                self.robot.Ctrl_Car(3, 1, abs(right_speed))
                                
                            # 计算舵机增量控制
                            # 根据右摇杆值计算舵机角度变化量
                            # 反转Y轴，以便向右为正
                            y_change = -right_y * SERVO_SPEED
                            # 反转X轴，以便向上为正
                            x_change = -right_x * SERVO_SPEED
                            
                            # 更新舵机角度
                            self.servo1_current_angle += y_change
                            self.servo2_current_angle += x_change
                            
                            # 限制角度范围
                            self.servo1_current_angle = max(0, min(180, self.servo1_current_angle))
                            self.servo2_current_angle = max(0, min(110, self.servo2_current_angle))
                            
                            # 控制舵机
                            self.robot.Ctrl_Servo(1, int(self.servo1_current_angle))
                            self.robot.Ctrl_Servo(2, int(self.servo2_current_angle))
                            
                            # 记录舵机角度
                            logger.info(f"舵机角度: 1号={int(self.servo1_current_angle)}, 2号={int(self.servo2_current_angle)}")
                            
                        except Exception as e:
                            logger.error(f"控制机器人时出错: {str(e)}")
                    
                    last_command_time = current_time
                elif current_time - last_command_time > timeout:
                    # 如果超时，停止机器人
                    logger.info("无数据，停止机器人")
                    self.stop_motors()
                            
            time.sleep(0.05)  # 50ms控制频率
        
        # 线程结束时停止机器人
        self.stop_motors()
        logger.info("机器人控制线程已停止")
    
    def stop_motors(self):
        """停止所有电机"""
        if raspbot_available and self.robot:
            try:
                for i in range(4):
                    self.robot.Ctrl_Car(i, 0, 0)
            except Exception as e:
                logger.error(f"停止机器人时出错: {str(e)}")
    
    def start_control(self):
        """启动机器人控制线程"""
        if self.control_thread_running:
            return False
            
        self.control_thread_running = True
        self.control_thread = threading.Thread(target=self.control_robot_thread, daemon=True)
        self.control_thread.start()
        return True
    
    def stop_control(self):
        """停止机器人控制线程"""
        if not self.control_thread_running:
            return False
            
        self.control_thread_running = False
        if self.control_thread:
            self.control_thread.join(timeout=2.0)
        return True
    
    def update_joystick_data(self, data):
        """更新最新的手柄数据并应用平滑处理"""
        # 应用平滑处理
        smoothed_data = self.input_smoother.smooth_input(data)
        self.latest_joystick = smoothed_data
    
    def is_running(self):
        """返回控制线程是否运行中"""
        return self.control_thread_running
    
    def is_available(self):
        """返回机器人是否可用"""
        return raspbot_available
